#include <pthread.h>
#include <iterator>

#include "tcpFinsCommand.h"
#include <pthread.h>
#include <string.h>

namespace OmronPlc
{
	tcpFinsCommand::tcpFinsCommand(uint8_t ServiceID)
	{
		// transport layer
		//
		transport = new tcpTransport();
		memset(cmdSend, 0, sizeof(cmdSend));
		tcpFinsCmd_S *pCmd = (tcpFinsCmd_S *)cmdSend;

		memcpy(pCmd->header.magic, "FINS", 4);
		pCmd->header.cmd[3] = 0x2;// Command FS Sending=2 / Receiving=3
		
		printf("sizeof(tcpFinsCmd_S) = %d \n",sizeof(tcpFinsCmd_S));
		// Default fins command fields
		//
		//---- COMMAND HEADER -------------------------------------------------------
		pCmd->feild[ICF] = 0x80;			// 00 ICF Information control field 
		pCmd->feild[RSC] = 0x00;			// 01 RSC Reserved 
		pCmd->feild[GTC] = 0x02;			// 02 GTC Gateway count
		pCmd->feild[DNA] = 0x00;			// 03 DNA Destination network address (0=local network)
		pCmd->feild[DA1] = 0x00;			// 04 DA1 Destination node number
		pCmd->feild[DA2] = 0x00;			// 05 DA2 Destination unit address
		pCmd->feild[SNA] = 0x00;			// 06 SNA Source network address (0=local network)
		pCmd->feild[SA1] = 0x00;			// 07 SA1 Source node number
		pCmd->feild[SA2] = 0x00;			// 08 SA2 Source unit address
		pCmd->feild[SID] = ServiceID;		// 09 SID Service ID
		//---- COMMAND --------------------------------------------------------------
		pCmd->feild[MC] = 0x00;				// 10 MC Main command
		pCmd->feild[SC] = 0x00;				// 11 SC Subcommand
		//---- PARAMS ---------------------------------------------------------------

		respFins.resize(2048); 
		respFinsData.resize(2048);

		finsCommandLen = 0;
		//导出respond
		pCmd = (tcpFinsCmd_S *)respFS;
		Response = pCmd->parameter + 2;//数据前面有两个字节的errorCode
		
	}

	tcpFinsCommand::~tcpFinsCommand()
	{
		delete transport;
	}

	bool tcpFinsCommand::Connect()
	{
		bool b = false;
		transport->Connect();
		b = NodeAddressDataSend();
		if(!b){
			std::cout << getLastError() << std::endl;
		}
		return b; 
	}

	void tcpFinsCommand::Close()
	{
		transport->Close();
	}

	void tcpFinsCommand::SetRemote(string ipaddr, uint16_t port)
	{
		transport->SetRemote(ipaddr, port);
	}

	bool tcpFinsCommand::MemoryAreaRead(MemoryArea area, uint16_t address, uint8_t bit_position, uint16_t count)
	{
		tcpFinsCmd_S *pCmd = (tcpFinsCmd_S *)cmdSend;
		pCmd->feild[MC] = 0x01;
		pCmd->feild[SC] = 0x01;

		tcpFinsReadCmd_S *pRead = (tcpFinsReadCmd_S *)pCmd->parameter;
		pRead->area = area;
		pRead->addr[0] = (uint8_t)((address >> 8) & 0xFF);
		pRead->addr[1] = (uint8_t)((address >> 0) & 0xFF);
		pRead->bitPos = (uint8_t)(bit_position);
		pRead->length[0] = (uint8_t)((count >> 8) & 0xFF);
		pRead->length[1] = (uint8_t)(count & 0xFF);

		finsCommandLen = 18;

		return FrameSend();
	}

	bool tcpFinsCommand::MemoryAreaWrite(MemoryArea area, uint16_t address, uint8_t bit_position, uint16_t count, uint8_t data[])
	{
		// command & subcomand
		//
		tcpFinsCmd_S *pCmd = (tcpFinsCmd_S *)cmdSend;
		pCmd->feild[MC] = 0x01;
		pCmd->feild[SC] = 0x02;

		// memory area
		//
		pCmd->feild[F_PARAM] = area;

		tcpFinsWriteCmd_S *pWrite = (tcpFinsWriteCmd_S *)pCmd->parameter;
		// address
		//
		pWrite->addr[0] = (uint8_t)((address >> 8) & 0xFF);
		pWrite->addr[1] = (uint8_t)((address >> 0) & 0xFF);
		pWrite->bitPos = bit_position;

		// count items
		//
		pWrite->length[0] = (uint8_t)((count >> 8) & 0xFF);
		pWrite->length[1] = (uint8_t)((count >> 0) & 0xFF);

		// set command lenght (12 + additional params)
		//
		finsCommandLen = 18 + count;

		memcpy(pWrite->value, data, count);

		// send the message
		//
		return FrameSend();
	}

	bool tcpFinsCommand::NodeAddressDataSend()
	{
		/* NODE ADDRESS DATA SEND buffer */
		uint8_t cmdNADS[] =
		{
		0x46, 0x49, 0x4E, 0x53, // 'F' 'I' 'N' 'S'
		0x00, 0x00, 0x00, 0x0C,	// 12 Bytes expected
		0x00, 0x00, 0x00, 0x00,	// NADS Command (0 Client to server, 1 server to client)
		0x00, 0x00, 0x00, 0x00,	// Error code (Not used)
		0x00, 0x00, 0x00, 0x00	// Client node address, 0 = auto assigned
		};
		tcpFinsCmd_S *pRead = (tcpFinsCmd_S *)cmdSend;


		// send NADS command
		//
		transport->Send(cmdNADS, sizeof(cmdNADS));

		// wait for a plc response
		//
		uint8_t respNADS[24] = { 0 };
		transport->Receive(respNADS, sizeof(respNADS));


		// checks response error
		//
		if (respNADS[15] != 0)
		{
			lastError = "NASD command error: ";
			lastError += respNADS[15];

			// no more actions
			//
			Close();
			return false;
		}


		// checking header error
		//
		if (respNADS[8] != 0 || respNADS[9] != 0 || respNADS[10] != 0 || respNADS[11] != 0)
		{
			lastError = "Error sending NADS command. ";
			lastError += respNADS[8];
			lastError += " ";
			lastError += respNADS[9];
			lastError += " ";
			lastError += respNADS[10];
			lastError += " ";
			lastError += respNADS[11];

			// no more actions
			//
			Close();

			return false;
		}


		// save the client & server node in the FINS command for all next conversations
		//
		pRead->feild[DA1] = respNADS[23];
		pRead->feild[SA1] = respNADS[19];

		return true;
	}

	bool tcpFinsCommand::FrameSend()
	{
		int fsLen = finsCommandLen + 8;
		tcpFinsCmd_S *pRead = (tcpFinsCmd_S *)cmdSend;
		tcpFinsCmd_S *pRet;
		
		pRead->header.len[2] = (uint8_t)((fsLen >> 8) & 0xFF);
		pRead->header.len[3] = (uint8_t)(fsLen & 0xFF);

		//frame send
		transport->Send(cmdSend, fsLen + 8);
		// frame response
		if(transport->Receive(&respFS[0], 0) < 0){
			printf("Recv data failed \n");
			return false;
		}
		pRet = (tcpFinsCmd_S*)respFS;

		// check frame error [8]+[9]+[10]+[11]
		string FSR_ERR = "";
		FSR_ERR += pRet->header.cmd[0];
		FSR_ERR += pRet->header.cmd[1];
		FSR_ERR += pRet->header.cmd[2];
		FSR_ERR += pRet->header.cmd[3];
		if (FSR_ERR.compare("0002") == 0)
		{
			lastError = "FRAME SEND error: " + FSR_ERR;
			return false;
		}

		// checks response error
		//
		if (pRet->header.errorCode[3] != 0)
		{
			lastError = "Error receving FS command: " + respFS[15];
			return false;
		}
		
		//check parameter error code
		if (pRet->parameter[0] != 0 || pRet->parameter[1] != 0)
		{
			lastError += "Response Code error: (Code: ";
			lastError += respFins[12];
			lastError += "Subcode: ";
			lastError += respFins[13];
			lastError += ")";

			return false;
		}

		return true;
	}
}
